/*
 * Copyright (c) 2020, yifengling0. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. No personal names or organizations' names associated with the
 *    Atomthreads project may be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE ATOMTHREADS PROJECT AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE PROJECT OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "atom.h"
#include "atommutex.h"
#include "gnss.h"
#include <string.h>
#include <math.h>
#include <stdio.h>
#include <ctype.h>
#include "pin.h"
#include "syslog.h"

#define GNSS_RX_PRI       	(12)

static void gnss_suspend(Device_t* dev)
{
	DeviceGnss* gnss = (DeviceGnss*)dev;
	gnss->Protected.disable(gnss);
}

static void gnss_resume(Device_t* dev)
{
	DeviceGnss* gnss = (DeviceGnss*)dev;
	gnss->Protected.enable(gnss);
}

static int hex_to_int(const char* start, const char* end)
{
	int len = end - start;
	int result = 0;
	int c;

	for (; len > 0; len--, start++)
	{
		if (start >= end)
			return 0;
		c = *start;
		if ((c >= '0') && (c <= '9'))
			c = c - '0';
		else if ((c >= 'A') && (c <= 'F'))
			c = c - 'A' + 10;
		else if ((c >= 'a') && (c <= 'f'))
			c = c - 'a' + 10;
		else
			return 0;
		result = result * 16 + c;
	}
	return result;
}

static int str_to_int(const char* start, const char* end)
{
	int len = end - start;
	int result = 0;
	int c;

	for (; len > 0; len--, start++)
	{
		if (start >= end)
			return 0;
		c = *start - '0';
		if (c >= 10)
			return 0;
		result = result * 10 + c;
	}
	return result;
}

static double latitude_longitude_convert(const char* s)
{
	double v = strtod(s, NULL);
	int d = (int)(floor(v) / 100);
	return d + (v - d * 100) / 60.0;
}

static inline void gnss_nmea_parse_gga(struct gnss_nmea_t* nmea)
{
	const char* p;

	if (nmea->tsize > 9)
	{
		if (nmea->tend[1] - nmea->tstart[1] >= 6)
		{
			p = nmea->tstart[1];
			nmea->utc.hour = str_to_int(p, p + 2);
			nmea->utc.minute = str_to_int(p + 2, p + 4);
			nmea->utc.second = str_to_int(p + 4, p + 6);
			switch (nmea->tend[1] - nmea->tstart[1])
			{
			case 8:
				nmea->utc.millisecond = str_to_int(p + 7, p + 8) * 100;
				break;
			case 9:
				nmea->utc.millisecond = str_to_int(p + 7, p + 9) * 10;
				break;
			case 10:
				nmea->utc.millisecond = str_to_int(p + 7, p + 10);
				break;
			default:
				nmea->utc.millisecond = 0;
				break;
			}
		}
		else
		{
			nmea->utc.hour = 0;
			nmea->utc.minute = 0;
			nmea->utc.second = 0;
			nmea->utc.millisecond = 0;
		}

		if (nmea->tend[2] - nmea->tstart[2] >= 6)
		{
			nmea->latitude = latitude_longitude_convert(nmea->tstart[2]);
			if (toupper(*nmea->tstart[3]) == 'S')
				nmea->latitude = -nmea->latitude;
		}
		else
			nmea->latitude = 0;

		if (nmea->tend[4] - nmea->tstart[4] >= 6)
		{
			nmea->longitude = latitude_longitude_convert(nmea->tstart[4]);
			if (toupper(*nmea->tstart[5]) == 'W')
				nmea->longitude = -nmea->longitude;
		}
		else
			nmea->longitude = 0;

		if (nmea->tend[6] - nmea->tstart[6] > 0)
			nmea->signal = str_to_int(nmea->tstart[6], nmea->tend[6]);
		else
			nmea->signal = GNSS_SIGNAL_INVALID;

		if (nmea->tend[7] - nmea->tstart[7] > 0)
			nmea->used = str_to_int(nmea->tstart[7], nmea->tend[7]);
		else
			nmea->used = 0;

		if (nmea->tend[8] - nmea->tstart[8] > 0)
			nmea->precision.hdop = strtod(nmea->tstart[8], NULL);
		else
			nmea->precision.hdop = 0;

		if (nmea->tend[9] - nmea->tstart[9] > 0)
			nmea->altitude = strtod(nmea->tstart[9], NULL);
		else
			nmea->altitude = 0;
	}
}

static inline void gnss_nmea_parse_rmc(struct gnss_nmea_t* nmea)
{
	const char* p;

	if (nmea->tsize > 10)
	{
		if (nmea->tend[1] - nmea->tstart[1] >= 6)
		{
			p = nmea->tstart[1];
			nmea->utc.hour = str_to_int(p, p + 2);
			nmea->utc.minute = str_to_int(p + 2, p + 4);
			nmea->utc.second = str_to_int(p + 4, p + 6);
			switch (nmea->tend[1] - nmea->tstart[1])
			{
			case 8:
				nmea->utc.millisecond = str_to_int(p + 7, p + 8) * 100;
				break;
			case 9:
				nmea->utc.millisecond = str_to_int(p + 7, p + 9) * 10;
				break;
			case 10:
				nmea->utc.millisecond = str_to_int(p + 7, p + 10);
				break;
			default:
				nmea->utc.millisecond = 0;
				break;
			}
		}
		else
		{
			nmea->utc.hour = 0;
			nmea->utc.minute = 0;
			nmea->utc.second = 0;
			nmea->utc.millisecond = 0;
		}

		if (nmea->tend[3] - nmea->tstart[3] >= 6)
		{
			nmea->latitude = latitude_longitude_convert(nmea->tstart[3]);
			if (toupper(*nmea->tstart[4]) == 'S')
				nmea->latitude = -nmea->latitude;
		}
		else
			nmea->latitude = 0;

		if (nmea->tend[5] - nmea->tstart[5] >= 6)
		{
			nmea->longitude = latitude_longitude_convert(nmea->tstart[5]);
			if (toupper(*nmea->tstart[6]) == 'W')
				nmea->longitude = -nmea->longitude;
		}
		else
			nmea->longitude = 0;

		if (nmea->tend[7] - nmea->tstart[7] > 0)
			nmea->speed = strtod(nmea->tstart[7], NULL) * 1.852;
		else
			nmea->speed = 0;

		if (nmea->tend[8] - nmea->tstart[8] > 0)
			nmea->track = strtod(nmea->tstart[8], NULL);
		else
			nmea->track = 0;

		if (nmea->tend[9] - nmea->tstart[9] >= 6)
		{
			p = nmea->tstart[9];
			nmea->utc.day = str_to_int(p, p + 2);
			nmea->utc.month = str_to_int(p + 2, p + 4);
			nmea->utc.year = str_to_int(p + 4, p + 6) + 2000;
		}
		else
		{
			nmea->utc.day = 1;
			nmea->utc.month = 1;
			nmea->utc.year = 2000;
		}

		if (nmea->tend[10] - nmea->tstart[10] > 0)
		{
			nmea->magvar = strtod(nmea->tstart[10], NULL);
			if (toupper(*nmea->tstart[11]) == 'W')
				nmea->magvar = -nmea->magvar;
		}
		else
			nmea->magvar = 0;
	}
}

static inline void gnss_nmea_parse_vtg(struct gnss_nmea_t* nmea)
{
	if (nmea->tsize > 7)
	{
		if (nmea->tend[1] - nmea->tstart[1] > 0)
			nmea->track = strtod(nmea->tstart[1], NULL);
		else
			nmea->track = 0;

		if (nmea->tend[3] - nmea->tstart[3] > 0)
			nmea->mtrack = strtod(nmea->tstart[3], NULL);
		else
			nmea->mtrack = 0;

		if (nmea->tend[7] - nmea->tstart[7] > 0)
			nmea->speed = strtod(nmea->tstart[7], NULL);
		else
			nmea->speed = 0;
	}
}

static inline void gnss_nmea_parse_gsa(struct gnss_nmea_t* nmea)
{
	if (nmea->tsize > 17)
	{
		if (nmea->tend[2] - nmea->tstart[2] > 0)
			nmea->fix = str_to_int(nmea->tstart[2], nmea->tend[2]);
		else
			nmea->fix = GNSS_FIX_NONE;

		if (nmea->tend[15] - nmea->tstart[15] > 0)
			nmea->precision.pdop = strtod(nmea->tstart[15], NULL);
		else
			nmea->precision.pdop = 0;

		if (nmea->tend[16] - nmea->tstart[16] > 0)
			nmea->precision.hdop = strtod(nmea->tstart[16], NULL);
		else
			nmea->precision.hdop = 0;

		if (nmea->tend[17] - nmea->tstart[17] > 0)
			nmea->precision.vdop = strtod(nmea->tstart[17], NULL);
		else
			nmea->precision.vdop = 0;
	}
}

static inline void gnss_nmea_parse_gpgsv(struct gnss_nmea_t* nmea)
{
	int total, n;
	int viewed, c;
	int i;

	if (nmea->tsize > 3)
	{
		total = str_to_int(nmea->tstart[1], nmea->tend[1]);
		n = str_to_int(nmea->tstart[2], nmea->tend[2]);
		viewed = MIN((int)str_to_int(nmea->tstart[3], nmea->tend[3]), (int)ITEM_NUM(nmea->satellite.gps.sv));
		if (n <= 1)
			c = 0;
		else
			c = (n - 1) * 4;
		for (i = 0; (i < 4) && (c < viewed); i++)
		{
			nmea->satellite.gps.sv[c].prn = str_to_int(nmea->tstart[i * 4 + 4], nmea->tend[i * 4 + 4]);
			nmea->satellite.gps.sv[c].elevation = str_to_int(nmea->tstart[i * 4 + 5], nmea->tend[i * 4 + 5]);
			nmea->satellite.gps.sv[c].azimuth = str_to_int(nmea->tstart[i * 4 + 6], nmea->tend[i * 4 + 6]);
			nmea->satellite.gps.sv[c].snr = str_to_int(nmea->tstart[i * 4 + 7], nmea->tend[i * 4 + 7]);
			c++;
		}
		if (n == total)
		{
			for (i = viewed; i < ITEM_NUM(nmea->satellite.gps.sv); i++)
			{
				nmea->satellite.gps.sv[i].prn = 0;
				nmea->satellite.gps.sv[i].elevation = 0;
				nmea->satellite.gps.sv[i].azimuth = 0;
				nmea->satellite.gps.sv[i].snr = 0;
			}
			nmea->satellite.gps.n = viewed;
		}
	}
}

static inline void gnss_nmea_parse_bdgsv(struct gnss_nmea_t* nmea)
{
	int total, n;
	int viewed, c;
	int i;

	if (nmea->tsize > 3)
	{
		total = str_to_int(nmea->tstart[1], nmea->tend[1]);
		n = str_to_int(nmea->tstart[2], nmea->tend[2]);
		viewed = MIN((int)str_to_int(nmea->tstart[3], nmea->tend[3]), (int)ITEM_NUM(nmea->satellite.beidou.sv));
		if (n <= 1)
			c = 0;
		else
			c = (n - 1) * 4;
		for (i = 0; (i < 4) && (c < viewed); i++)
		{
			nmea->satellite.beidou.sv[c].prn = str_to_int(nmea->tstart[i * 4 + 4], nmea->tend[i * 4 + 4]);
			nmea->satellite.beidou.sv[c].elevation = str_to_int(nmea->tstart[i * 4 + 5], nmea->tend[i * 4 + 5]);
			nmea->satellite.beidou.sv[c].azimuth = str_to_int(nmea->tstart[i * 4 + 6], nmea->tend[i * 4 + 6]);
			nmea->satellite.beidou.sv[c].snr = str_to_int(nmea->tstart[i * 4 + 7], nmea->tend[i * 4 + 7]);
			c++;
		}
		if (n == total)
		{
			for (i = viewed; i < ITEM_NUM(nmea->satellite.beidou.sv); i++)
			{
				nmea->satellite.beidou.sv[i].prn = 0;
				nmea->satellite.beidou.sv[i].elevation = 0;
				nmea->satellite.beidou.sv[i].azimuth = 0;
				nmea->satellite.beidou.sv[i].snr = 0;
			}
			nmea->satellite.beidou.n = viewed;
		}
	}
}

static inline uint32_t shash(const char* s)
{
	uint32_t v = 5381;
	if (s)
	{
		while (*s)
			v = (v << 5) + v + (*s++);
	}
	return v;
}

static int DeviceGnssUpdate(DeviceGnss* gnss)
{
	struct gnss_nmea_t* nmea = NULL;
	unsigned char cksum;
	char buf[256];
	int n, i, j;
	int update = 0;

	if (gnss && gnss->Protected.dev_read)
	{
		nmea = &gnss->Protected.nmea;
		while ((n = gnss->Protected.dev_read(gnss, buf, sizeof(buf))) > 0)
		{
			for (i = 0; i < n; i++)
			{
				switch (buf[i])
				{
				case '$':
					nmea->bindex = 0;
					nmea->dindex = 0;
					nmea->tsize = 0;
					break;

				case ',':
				case '*':
				case '\r':
					nmea->tstart[nmea->tsize] = &nmea->buffer[nmea->bindex - nmea->dindex];
					nmea->tend[nmea->tsize] = &nmea->buffer[nmea->bindex];
					nmea->tsize++;
					nmea->buffer[nmea->bindex] = buf[i];
					nmea->bindex++;
					nmea->dindex = 0;
					break;

				case '\n':
					if (nmea->tsize > 2)
					{
						for (j = 0, cksum = 0; j < nmea->tend[nmea->tsize - 2] - nmea->tstart[0]; j++)
							cksum ^= nmea->buffer[j];
						if (cksum == hex_to_int(nmea->tstart[nmea->tsize - 1], nmea->tend[nmea->tsize - 1]))
						{
							for (j = 0; j < nmea->tsize; j++)
								*nmea->tend[j] = 0;
							switch (shash(nmea->tstart[0]))
							{
							case 0x0d30e1ab: /* "gpgga" */
							case 0x0ccfd29a: /* "bdgga" */
							case 0x0d2fc8e9: /* "gngga" */
								gnss_nmea_parse_gga(nmea);
								break;

							case 0x0d31113e: /* "gprmc" */
							case 0x0cd0022d: /* "bdrmc" */
							case 0x0d2ff87c: /* "gnrmc" */
								gnss_nmea_parse_rmc(nmea);
								break;

							case 0x0d31232d: /* "gpvtg" */
							case 0x0cd0141c: /* "bdvtg" */
							case 0x0d300a6b: /* "gnvtg" */
								gnss_nmea_parse_vtg(nmea);
								break;

							case 0x0d30e337: /* "gpgsa" */
							case 0x0ccfd426: /* "bdgsa" */
							case 0x0d2fca75: /* "gngsa" */
								gnss_nmea_parse_gsa(nmea);
								break;

							case 0x0d30e34c: /* "gpgsv" */
								gnss_nmea_parse_gpgsv(nmea);
								break;

							case 0x0ccfd43b: /* "bdgsv" */
								gnss_nmea_parse_bdgsv(nmea);
								break;

							default:
								break;
							}
							update = 1;
						}
					}
					nmea->bindex = 0;
					nmea->dindex = 0;
					nmea->tsize = 0;
					break;

				default:
					nmea->buffer[nmea->bindex] = buf[i];
					nmea->bindex++;
					nmea->dindex++;
					break;
				}
			}
		}
	}
	return update;
}


static const struct gnss_nmea_t* DeviceGnssNmea(DeviceGnss* gnss)
{
	return &gnss->Protected.nmea;
}

static void thread_for_gnss(void* param)
{
	DeviceGnss * gnss = (DeviceGnss*)param;

	while (1)
	{
		if( DeviceGnssUpdate(gnss)){
			const struct gnss_nmea_t *nmea = DeviceGnssNmea(gnss);

			if(gnss->Protected.ops != NULL && gnss->Protected.ops->data_ready_event != NULL){
				gnss->Protected.ops->data_ready_event(gnss, nmea);
			}

			atomMutexGet(&gnss->Data.lock, 0);
			memcpy(&gnss->Data.data, nmea, sizeof(struct gnss_nmea_t));
			atomMutexPut(&gnss->Data.lock);
		}else{
			atomTimerDelay(50);
		}
	}
	
}

void DeviceGnssGetData(DeviceGnss* gnss, struct gnss_nmea_t *data)
{
	atomMutexGet(&gnss->Data.lock, 0);

	memcpy(data, &gnss->Data.data, sizeof(struct gnss_nmea_t));

	atomMutexPut(&gnss->Data.lock);
}

static void gnss_reset(DeviceGnss* gnss)
{
	PinMode(gnss->Protected.resetPin, PIN_MODE_OUTPUT);
	PinWrite(gnss->Protected.resetPin, PIN_LOW);
	atomTimerDelay(50);
	PinWrite(gnss->Protected.resetPin, PIN_HIGH);
	atomTimerDelay(50);
}

void DeviceGnssCreate(DeviceGnss* gnss, const DeviceDesc *desc, const GnssOps* ops)
{
	gnss->Protected.ops = ops;
	gnss->super.resume = gnss_resume;
	gnss->super.suspend = gnss_suspend;
	gnss->super.desc = desc;

	DeviceInstall(&gnss->super, desc);

	gnss_reset(gnss);

	gnss->Protected.init(gnss);

	atomThreadCreate(gnss->Protected.tcb, GNSS_RX_PRI, thread_for_gnss, (void*)gnss, gnss->Protected.stack, GNSS_STACK_SIZE, TRUE);

	SYSLOG(LOG_INFO, "device %s initlized", desc->name);
}